;/**
; *  cpu_a.s - All calls(except init_stack) defined in cpu.h are implemented.
; *
; *  Copyright (C) 2008  ZhangHu
; *  All rights reserved.
; *  E-MAIL: anmnmnly@gmail.com
; *
; *  This program is free software: you can redistribute it and/or modify
; *  it under the terms of the GNU General Public License as published by
; *  the Free Software Foundation, either version 3 of the License, or
; *  (at your option) any later version.
; *
; *  This program is distributed in the hope that it will be useful,
; *  but WITHOUT ANY WARRANTY; without even the implied warranty of
; *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
; *  GNU General Public License for more details.
; *
; *  You should have received a copy of the GNU General Public License
; *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
; */


    AREA  |SUBR|,CODE,READONLY

    IMPORT pcurtcb
    IMPORT pnewtcb

    GBLA BUILD_MODE
BUILD_MODE SETA 1        ;0=O0 1=O1 or O2

    EXPORT  DisableInt
DisableInt
    STMDB SP!,{R0}
    MRS   R0,CPSR
    ORR   R0,R0,#0x80
    MSR   CPSR_cxsf,R0
    LDMIA SP!,{R0}
    MOV   PC,LR

    EXPORT  EnableInt
EnableInt
    STMDB SP!,{R0}
    MRS   R0,CPSR
    BIC   R0,R0,#0x80
    MSR   CPSR_cxsf,R0
    LDMIA SP!,{R0}
    MOV   PC,LR

    EXPORT TaskIntStart
TaskIntStart
    MRS R0,SPSR              ;switch sys_mode and disable interrupt
    ORR R0,R0,#0xDF
    MSR CPSR_cxsf,R0
    B TaskStart

    EXPORT TaskIntSw
TaskIntSw
    LDR R0,=pcurtcb          ;get context of tcb_t
    LDR R0,[R0]
    LDR R0,[R0]

    [ BUILD_MODE=0           ;Debug build
        ADD SP,SP,#8
        LDMFD SP!,{R4-R6}    ;save R4-R6
        ADD SP,SP,#4
    ]
    [ BUILD_MODE=1           ;DebugRel or release build
        ADD SP,SP,#4
        LDMFD SP!,{R4-R5}    ;save R4, R5
        ADD SP,SP,#4
    ]

    SUB R0,R0,#44
    STMIA R0!,{R4-R11}       ;save R4-R11

    SUB R0,R0,#52

    MRS R4,SPSR
    STR R4,[R0]              ;save CPSR_sys

    ADD R4,R0,#4

    LDMFD SP!,{R0-R3,R12,LR}
    STMIA R4!,{R0-R3}        ;save R0-R3

    ADD R4,R4,#32
    STMIA R4!,{R12}          ;save R12

    ADD R4,R4,#8
    SUBS LR,LR,#4
    STR LR,[R4]              ;save PC

    MRS R0,SPSR
    MSR CPSR_cxsf,R0         ;switch to system mode

    SUB R4,R4,#68
    MRS R0,SPSR
    STR R0,[R4]              ;save SPSR

    STR SP,[R4,#60]          ;save SP
    STR LR,[R4,#64]          ;save LR
    ;All task contexts have been stored.

    B TaskStart

    EXPORT TaskSwitch
TaskSwitch
    STMFD SP!,{R0-R2}        ;save R0-R2

    LDR R0,=pcurtcb          ;get context of tcb_t
    LDR R0,[R0]
    LDR R0,[R0]

    MRS R1,SPSR
    STR R1,[R0,#-64]         ;save CPSR
    STR LR,[R0]              ;save PC

	MOV R2,R0

    SUB R0,R0,#48
    STMIA R0!,{R3-R12}       ;save R3-R12

    SUB R3,R2,#60
    LDMFD SP!,{R0-R2}
    STMIA R3!,{R0-R2}        ;save R0-R2

    MRS R1,SPSR
    MSR CPSR_cxfs,R1         ;sys

    STR LR,[R3,#44]          ;save LR

    STR SP,[R3,#40]          ;save sp

    MRS R1,SPSR
    SUB R3,R3,#20
    STR R1,[R3]              ;save SPSR
    ;All task contexts have been stored.

    EXPORT TaskStart
TaskStart
    LDR   R6,=pnewtcb
    LDR   R6,[R6]
    LDR   R4,=pcurtcb        ;pcurtcb = pnewtcb
    STR   R6,[R4]

    LDR   R6,[R6]            ;R6 = context.

    LDR   R0,[R6,#-68]
    MSR   SPSR_cxfs,R0       ;restore SPSR

    LDR   SP,[R6,#-8]        ;restore SP

    LDR   R0,[R6]
    STMFD SP!,{R0}           ;push PC to statck

    LDR   R0,[R6,#-60]
    STMFD SP!,{R0}           ;push R0 to statck

    MOV   R0,R6
    SUB   R0,R0,#56
    LDMIA R0!,{R1-R12}       ;restore R1-R12

    LDR   LR,[R0,#4]         ;restore LR

    LDR   R0,[R0,#-56]
    MSR   CPSR_cxfs,R0       ;restore CPSR

    LDMFD SP!,{R0,PC}        ;start new task

    EXPORT TaskContinue
TaskContinue
	[ BUILD_MODE=0			 ;restore R4-R6
		ADD   SP,SP,#8
		LDMFD SP!,{R4-R6}
	]

    [ BUILD_MODE=1          ;DebugRel or Release build
		ADD   SP,SP,#4
		LDMFD SP!,{R4-R5}	;restore R4, R5
	]

	ADD   SP,SP,#24
    LDR   R0,[SP]
    SUBS  R0,R0,#4
    STR   R0,[SP]

    SUB   SP,SP,#20
    LDMFD SP!,{R0-R3,R12,PC}^

    END

